package com.jmex.physics.impl.bullet.joints;

import java.util.ArrayList;
import java.util.Collections;
import java.util.List;

import com.jme.math.Quaternion;
import com.jme.math.Vector3f;
import com.jme.scene.Node;
import com.jmex.physics.DynamicPhysicsNode;
import com.jmex.physics.Joint;
import com.jmex.physics.JointAxis;
import com.jmex.physics.PhysicsSpace;
import com.jmex.physics.RotationalJointAxis;
import com.jmex.physics.TranslationalJointAxis;
import com.jmex.physics.impl.bullet.BulletDynamicPhysicsNode;
import com.jmex.physics.impl.bullet.BulletNativeInvoker;
import com.jmex.physics.impl.bullet.BulletPhysicsSpace;

public class BulletJoint extends Joint {

	public enum Type {
		Fixed,
		Slider,
		Hinge,
		Hinge2,
		ConeTwist,
		SixDOF
	}
	
	private Vector3f anchor = new Vector3f();
    private boolean isDirty = true;
    private BulletPhysicsSpace space;
    private BulletDynamicPhysicsNode[] nodes = new BulletDynamicPhysicsNode[2];
    
    private List<BulletDynamicPhysicsNode> nodeList = new ArrayList<BulletDynamicPhysicsNode>();
    private boolean collisionEnabled = false;
    
    private long nativeBulletHandle=-1;
    
	private Type jointType = Type.Fixed;
    
    private Vector3f transA = new Vector3f();
    private Vector3f transB = new Vector3f();
    private Quaternion rotA = new Quaternion();
    private Quaternion rotB = new Quaternion();

    public BulletJoint( BulletPhysicsSpace space ) {
        this.space = space;
    }

    public void buildConstraint()
    {
    	isDirty=false;
    	
    	//this is a nonsensical situation, so we'll just ignore it for now.
        if ( nodes[0] == null ) {
            return;
        }

        //count the axes and determine the type of joint.
        int numTranslational = 0;
        int numRotational = 0;
        int index = 0;
        JointAxis[] axes = new JointAxis[3];
        Vector3f[] directions = new Vector3f[3];
        for ( JointAxis axis : getAxes() ) {
        	if(index>2)
        		unsupported("Currently, joints with more than 3 axes of ANY type are unsupported.");
            if ( axis.isTranslationalAxis() )
                numTranslational++;
            if ( axis.isRotationalAxis() )
                numRotational++;
            axes[index] = axis;
            directions[index] = axis.getDirection(directions[index]);
            directions[index].normalizeLocal();
            index++;
        }
    	
        if(numTranslational==0 && numRotational==0)
        {
        	jointType=Type.Fixed;
        }
        else if(numTranslational==1 && numRotational==0)
        {
        	jointType=Type.Slider;
        }
        else if(numTranslational==0 && numRotational==1)
        {
        	jointType=Type.Hinge;
        }
        else if(numTranslational==0 && numRotational==2)
        {
        	if(axes[0].isRelativeToSecondObject() != axes[1].isRelativeToSecondObject())
        	{
        		jointType=Type.Hinge2;
        	}
        	else
        	{
        		unsupported("Unfortunately, the cone-twist joint is not yet implemented.");
//        		jointType=Type.ConeTwist;
        	}
        }
        else if(numTranslational<=3 && numRotational<=3)
        {
        	jointType=Type.SixDOF;
        }
        else
        	unsupported("Joints can only have 3 axes of each type.");

        //figure out where the anchor goes.
        Vector3f anchor = new Vector3f();
        if ( nodes[1] == null ) {
        	//Only one body, so the anchor is already in world coords.
        	anchor = getAnchor( anchor );
        } else {
        	//Anchor is relative to body A.  Translate to world coords.
        	anchor = getAnchor( anchor );
        	localToWorldIgnoreScale(nodes[0],anchor,anchor);
        }

        Quaternion q = new Quaternion();
        switch(jointType)
        {
        	case Slider:
        		prepareRotation(directions[0],Vector3f.UNIT_X,q);
        	break;
        	case Hinge:
	        	prepareRotation(directions[0],Vector3f.UNIT_Z,q);
	        break;
        	case Hinge2:
        	break;
        	case ConeTwist:
        	case SixDOF:
                prepareRotation(new Vector3f[]{directions[0],directions[1],directions[2]},q);
            break;
        }

        if(jointType.equals(Type.Hinge2))
        {
        	prepareRotation(directions[0],Vector3f.UNIT_X,rotA);
        	prepareRotation(directions[1],Vector3f.UNIT_X,rotB);
        	transA.set(anchor);
        } else {
	        rotA.set(q);
	        Vector3f tempCM=new Vector3f();
	        worldToLocalIgnoreScale(nodes[0],anchor, anchor);
	    	anchor.subtractLocal(nodes[0].getCenterOfMass(tempCM));
	    	transA.set(anchor);
	    	anchor.addLocal(tempCM);
	    	localToWorldIgnoreScale(nodes[0],anchor, anchor);
	        if(nodes[1]!=null)
	        {
	        	worldToLocalIgnoreScale(nodes[1],anchor, anchor);
	        	anchor.subtractLocal(nodes[1].getCenterOfMass(tempCM));
	        }
	        rotB.set(q);
	        transB.set(anchor);
        }
        
        nativeBulletHandle = BulletNativeInvoker.buildJoint(this, jointType.ordinal(),
        		rotA.x, rotA.y, rotA.z, rotA.w, 
        		transA.x, transA.y, transA.z, 
        		rotB.x, rotB.y, rotB.z, rotB.w, 
        		transB.x, transB.y, transB.z, 
        		(nodes[0]==null || nodes[0].getBody()==null)?-1:nodes[0].getBody().getNativeBulletHandle(), 
        		(nodes[1]==null || nodes[1].getBody()==null)?-1:nodes[1].getBody().getNativeBulletHandle(),
        		collisionEnabled);
    }    
    
    
    private Vector3f localToWorldIgnoreScale(Node node, final Vector3f in, Vector3f store) {
        if (store == null)
            store = new Vector3f();
        return node.getWorldRotation().mult(
                store.set(in), store).addLocal(
                node.getWorldTranslation());
    }
    
    private Vector3f worldToLocalIgnoreScale(Node node, final Vector3f in, final Vector3f store) {
        in.subtract(node.getWorldTranslation(), store);
        node.getWorldRotation().inverse().mult(store, store);
        return store;
    }

    private Quaternion prepareRotation(Vector3f[] axes, Quaternion result)
    {
    	if(result==null)
    		result = new Quaternion();
    	
    	int totalAxes=0;
    	for(Vector3f vec : axes)
    		if(vec!=null) totalAxes++;
    	
    	if(totalAxes==1)
    	{
    		if(axes[0]!=null)
    			return prepareRotation(axes[0],Vector3f.UNIT_X,result);
    		if(axes[1]!=null)
    			return prepareRotation(axes[0],Vector3f.UNIT_Y,result);
    		if(axes[2]!=null)
    			return prepareRotation(axes[0],Vector3f.UNIT_Z,result);
    	}
    	else if(totalAxes==2)
    	{
    		if(axes[0]==null)
    			result.fromAxes(axes[1].cross(axes[2]), axes[1], axes[2]);
    		if(axes[1]==null)
    			result.fromAxes(axes[0], axes[2].cross(axes[0]), axes[2]);
    		if(axes[2]==null)
    			result.fromAxes(axes[0], axes[1], axes[0].cross(axes[1]));
    	}
    	else if(totalAxes==3)
    	{
            result.fromAxes( axes[0], axes[1], axes[2] );
        }
    	
    	return result;
    }
    
    private Quaternion prepareRotation(Vector3f target, Vector3f start, Quaternion result)
    {
    	if(result==null)
    		result = new Quaternion();
    	
        float angleBetween = start.angleBetween( target );
        Vector3f rotationalAxis = start.cross( target );
        result.fromAngleAxis(angleBetween, rotationalAxis);

        return result;
    }
    
    private void unsupported( String message ) {
        setActive( false ); // to allow next update to succeed
        throw new UnsupportedOperationException( message + " Problematic Joint: " + this );
    }

    @Override
    public void attach( DynamicPhysicsNode leftNode, DynamicPhysicsNode rightNode ) {
        if ( !( leftNode instanceof BulletDynamicPhysicsNode ) ||
                !( rightNode instanceof BulletDynamicPhysicsNode ) ) {
            throw new IllegalArgumentException( "Cannot attach nodes from a non-JBullet physics space." );
        }

        nodes[0] = (BulletDynamicPhysicsNode) leftNode;
        nodes[1] = (BulletDynamicPhysicsNode) rightNode;

        isDirty = true;
    }

    @Override
    public void attach( DynamicPhysicsNode node ) {
        if ( !( node instanceof BulletDynamicPhysicsNode ) ) {
            throw new IllegalArgumentException( "Cannot attach nodes from a non-JBullet physics space." );
        }

        nodes[0] = (BulletDynamicPhysicsNode) node;

        isDirty = true;
    }

    @Override
    protected void added( JointAxis axis ) {
        super.added( axis );
        isDirty=true;
    }

    @Override
    public void removed( JointAxis axis ) {
        super.removed( axis );
        isDirty=true;
    }

    @Override
    protected RotationalJointAxis createRotationalAxisImplementation() {
        return new BulletRotationalJointAxis();
    }

    @Override
    protected TranslationalJointAxis createTranslationalAxisImplementation() {
        return new BulletTranslationalJointAxis();
    }

    @Override
    public void detach() {
        nodes[0] = null;
        nodes[1] = null;

        isDirty = true;
    }

    @Override
    public Vector3f getAnchor( Vector3f store ) {
        if(store==null)
        {
        	store = new Vector3f();
        }
        store.set( anchor );
        return store;
    }

    @Override
    public float getBreakingLinearForce() {
        // TODO Auto-generated method stub
        return 0;
    }

    @Override
    public float getBreakingTorque() {
        // TODO Auto-generated method stub
        return 0;
    }

    @Override
    public float getDampingCoefficient() {
        // TODO Auto-generated method stub
        return 0;
    }

    @Override
    public List<? extends DynamicPhysicsNode> getNodes() {
        if ( nodeList == null ) {
            nodeList = new ArrayList<BulletDynamicPhysicsNode>();
        }
        nodeList.clear();
        if ( nodes[0] != null ) {
            nodeList.add( nodes[0] );
        }
        if ( nodes[1] != null ) {
            nodeList.add( nodes[1] );
        }
        return Collections.unmodifiableList( nodeList );
    }

    public float getPosition(JointAxis axis)
    {
    	return 0f;
    }
    
    @Override
    public PhysicsSpace getSpace() {
        return this.space;
    }

    @Override
    public float getSpringConstant() {
        // TODO Auto-generated method stub
        return 0;
    }

    @Override
    public boolean isCollisionEnabled() {
    	return collisionEnabled;
    }

    @Override
    public void reset() {

        isDirty = true;
    }

    @Override
    public void setAnchor( Vector3f anchor ) {
        this.anchor.set( anchor );
        isDirty = true;
    }

    @Override
    public void setBreakingLinearForce( float breakingLinearForce ) {

        isDirty = true;
    }

    @Override
    public void setBreakingTorque( float breakingTorque ) {

        isDirty = true;
    }

    public void setDirty(boolean isDirty) {
		this.isDirty = isDirty;
	}

	@Override
    public void setCollisionEnabled( boolean enabled ) {
		if(enabled==collisionEnabled) return;
		collisionEnabled = enabled;		
        isDirty = true;
    }

    @Override
    public void setSpring( float springConstant, float dampingCoefficient ) {
        isDirty = true;
    }

    public boolean isDirty() {
        return isDirty;
    }

    public long getNativeBulletHandle() {
		return nativeBulletHandle;
	}

    public void updateJointAxes()
    {
    	JointAxis axis;
    	for(int loop=0; loop<getAxes().size(); loop++)
    	{
    		if(nativeBulletHandle==-1)
    			return;
    		axis=getAxes().get(loop);
    		if((axis.isRotationalAxis() && ((BulletRotationalJointAxis)axis).isDirty()) ||
    			(axis.isTranslationalAxis() && ((BulletTranslationalJointAxis)axis).isDirty()))
    		{
    			BulletNativeInvoker.updateJointAxisInfo(nativeBulletHandle, loop, 
    					axis.getAvailableAcceleration(), axis.getDesiredVelocity(), 
    					axis.getPositionMinimum(), axis.getPositionMaximum());
    			if(axis.isRotationalAxis())
    				((BulletRotationalJointAxis)axis).setDirty(false);
    			else
    				((BulletTranslationalJointAxis)axis).setDirty(false);
    		}
    	}
    }
}
